﻿
// 2023/11/15: 基于v6 2023/10/30版本。新增z轴相对速度、绝对速度、相对加速度、绝对加速度字段
// 2024/7/3: 基于新样本框架。枚举改为使用enum class
// 2025/4/7： 实现插值
// 2025/4/8： 修正color字段解析
// 2025/8/4: 不使用cat宏
// 2025/8/7: 新增ObjectClass.WheelBarrow, TruckHead, TrafficSign, TrafficLight

#ifndef OBJ_SENSOR_SAMPLE_V7_H
#define OBJ_SENSOR_SAMPLE_V7_H

#include "spadas.h"

namespace obj_sensor_sample_v7
{
	using namespace spadas;

	enum class PositionMode
	{
		ClosestPoint = 1, // 最近点
		BoxCenter = 2, // 框中心
	};

	enum class ObjectClass
	{
		General = 1, // 一般物体大类
		Car = 2, // 车辆大类
		Ped = 3, // 行人大类
		Truck = 4, // 货车大类
		Bike = 5, // Bike大类
		RailCar = 6, // 轨道车辆大类
		Special = 7, // 特殊物体大类
		RoadStatic = 8, // 道路内静态物体大类
		SideStatic = 9, // 道路外静态物体大类

		GeneralSmall = 11, // 一般小物体
		GeneralBig = 12, // 一般大物体
		Barrier = 13, // 一般障碍物

		Van = 21, // 面包车
		Minibus = 22, // 小巴
		Bus = 23, // 大巴
		BatteryCart = 24, // 园区电瓶车
		TinyCar = 25, // 微型车
		SUV = 26, // SUV

		Adult = 31, // 成人
		Child = 32, // 小孩
		Scooter = 33, // 平衡车
		WheelChair = 34, // 轮椅
		WheelBarrow = 35, // 手推车

		Minitruck = 41, // 小卡车
		ContainerTruck = 42, // 货柜车（货柜车头 + 拖车）
		SpecialCar = 43, // 特种车辆
		Trailer = 44, // 拖车
		TruckHead = 45, // 货柜车头

		Motorbike = 51, // 摩托车
		Bicycle = 52, // 自行车
		ElectricBike = 53, // 电瓶自行车
		Tricycle = 54, // 三轮车

		Train = 61, // 火车
		Tram = 62, // 有轨电车

		Animal = 71, // 动物
		Ball = 72, // 球类
		Litter = 73, // 垃圾等杂物

		Cone = 81, // 锥形路障
		ManholeCover = 82, // 井盖
		Patch = 83, // 路面补丁
		Gantry = 84, // 龙门架

		Pole = 91, // 竖杆
		Tree = 92, // 树木
		Vegetation = 93, // 灌木
		Building = 94, // 建筑物
		TrafficSign = 95, // 交通标志
		TrafficLight = 96, // 交通灯
	};

	struct ObjectColor
	{
		Bool valid;
		Byte r; // Red component of the object's color / 目标物的颜色R分量
		Byte g; // Green component of the object's color / 目标物的颜色g分量
		Byte b; // Blue component of the object's color / 目标物的颜色b分量

		ObjectColor() : valid(FALSE), r(0), g(0), b(0)
		{}
	};

	struct ObjectInfo
	{
		// Tracking info / 跟踪信息
		Int id; // ID of the object / 目标物ID
		Int age; // Age of the object / 目标物的Age
		OptionalInt rawID; // Raw ID of the object / 目标物的原始ID
		OptionalInt rawAge; // Raw age of the object / 目标物的原始Age
		OptionalDouble confidence; // [%] Confidence (existence) of the object / 目标物的置信度

		// Time related / 时间偏差
		OptionalInt timeOffset; // [us] Time offset (to the frame's time) / 时间偏置(相对于帧的时间戳)

		// Color info / 颜色信息
		ObjectColor color; // Color of the object / 目标物的颜色

		// Class related / 类别信息
		OptionalInt rawClassID; // Raw classification ID of the object / 目标物的原始类别ID
		ObjectClass classs; // Classification of the object / 目标物的类别
		OptionalDouble classConfidence; // [%] Confidence (classification) of the object / 目标物的分类置信度

		// Position ("pos" differs by PositionMode, "cp" is always closest point) / 位置信息(pos根据PositionMode意义不同，cp专指ClosestPoint)
		PositionMode posMode; // Position mode of the object 目标物的位置模式
		Double posx; // [m] X-axis coordination of the object's position / 目标物的x轴方向位置
		Double posy; // [m] Y-axis coordination of the object's position / 目标物的y轴方向位置
		Double posz; // [m] Z-axis coordination of the object's position / 目标物的z轴方向位置
		OptionalDouble posxSigma; // [m] X-axis sigma of the object's position / 目标物x轴方向位置的精度
		OptionalDouble posySigma; // [m] Y-axis sigma of the object's position / 目标物y轴方向位置的精度
		OptionalDouble poszSigma; // [m] Z-axis sigma of the object's position / 目标物z轴方向位置的精度
		Double cpx; // [m] X-axis coordination of the object's closest point / 目标物的最近点x轴坐标
		Double cpy; // [m] Y-axis coordination of the object's closest point / 目标物的最近点y轴坐标
		Double cpd; // [m] Distance between the object's closest point and subject vehicle's contour / 目标物的最近点与本车轮廓距离

		// Size, heading, trajectory / 尺寸, 朝向, 轨迹
		OptionalDouble width; // [m] Width of the object's box / 目标物的宽度
		OptionalDouble length; // [m] Length of the object's box / 目标物的长度
		OptionalDouble height; // [m] Height of the object's box / 目标物的高度
		OptionalDouble heading; // [deg] Orientation of the object / 目标物的朝向
		Array<Point2D> trajectory; // List of trajectory points / 目标物轨迹点的列表

		// Velocity, acceleration / 速度, 加速度
		OptionalDouble vxRel; // [KPH] X-axis relative velocity of the object / x轴方向相对速度
		OptionalDouble vxAbs; // [KPH] X-axis absolute velocity of the object / x轴方向绝对速度
		OptionalDouble vyRel; // [KPH] Y-axis relative velocity of the object / y轴方向相对速度
		OptionalDouble vyAbs; // [KPH] Y-axis absolute velocity of the object / y轴方向绝对速度
		OptionalDouble vzRel; // [KPH] Z-axis relative velocity of the object / z轴方向相对速度
		OptionalDouble vzAbs; // [KPH] Z-axis absolute velocity of the object / z轴方向绝对速度
		OptionalDouble axRel; // [m/s²] X-axis relative acceleration of the object / x轴方向相对加速度
		OptionalDouble axAbs; // [m/s²] X-axis absolute acceleration of the object / x轴方向绝对加速度
		OptionalDouble ayRel; // [m/s²] Y-axis relative acceleration of the object / y轴方向相对加速度
		OptionalDouble ayAbs; // [m/s²] Y-axis absolute acceleration of the object / y轴方向绝对加速度
		OptionalDouble azRel; // [m/s²] Z-axis relative acceleration of the object / z轴方向相对加速度
		OptionalDouble azAbs; // [m/s²] Z-axis absolute acceleration of the object / z轴方向绝对加速度

		// Turning / 转向
		OptionalDouble yawRate; // [deg/s] Yaw rate of the object / 横摆角速度
		OptionalDouble curvature; // [1/m] Turning curvature of the object / 转弯曲率

		// Contour info / 轮廓信息
		Array<Point2D> contour; // List of contour points / 目标物轮廓点的列表

		// Others / 其他信息
		OptionalDouble rcs; // [dB] Radar-cross-section of the radar target / 雷达目标物的雷达散射截面
		OptionalDouble vLng; // [m/s] Longitudinal velocity of the radar target / 雷达目标物的径向速度
		OptionalDouble vLat; // [m/s] Lateral velocity of the radar target / 雷达目标物的切向速度

		ObjectInfo()
		{
			id = 0;
			age = 0;
			classs = ObjectClass::General;
			posMode = PositionMode::ClosestPoint;
			posx = posy = posz = 0;
			cpx = cpy = cpd = 0;
		}

		Point2D position()
		{
			return Point2D((Float)posx, (Float)posy);
		}

		Point2D closestPoint()
		{
			return Point2D((Float)cpx, (Float)cpy);
		}

		OptionalDouble speed()
		{
			if (vxAbs.valid && vyAbs.valid)
			{
				if (vzAbs.valid) return math::root(math::square(vxAbs.value) + math::square(vyAbs.value) + math::square(vzAbs.value));
				else return math::root(math::square(vxAbs.value) + math::square(vyAbs.value));
			}
			else return OptionalDouble();
		}

		ObjectClass category()
		{
			if ((UInt)classs < 10) return classs;
			else return (ObjectClass)((UInt)classs / 10);
		}
	};

	struct ObjectSensorFov
	{
		Double positionX; // [m] X-axis coordination of the FOV's center point / FOV中心点x轴坐标
		Double positionY; // [m] Y-axis coordination of the FOV's center point / FOV中心的y轴坐标
		Double angleRange; // [deg] Angle range of the FOV / FOV的角度范围
		Double orientation; // [deg] Central heading angle of the FOV FOV中轴线朝向角
		Double distanceRange; // [m] Detect range of the FOV / FOV探测距离范围
		Double blindRange; // [m] Blind range of the FOV / FOV盲区范围

		ObjectSensorFov()
		{
			positionX = 0;
			positionY = 0;
			angleRange = 90;
			orientation = 0;
			distanceRange = 100;
			blindRange = 0;
		}
	};

	struct ObjectSensorSampleData
	{
		// Object info / 目标物信息
		Array<ObjectInfo> objects; // List of objects / 目标物列表
		OptionalInt koFrontIndex; // Index of CIPV object / 前向关键目标序号
		OptionalInt koLeftIndex; // Index of left side key object / 左侧关键目标序号
		OptionalInt koRightIndex; // Index of right side key object / 右侧关键目标序号

		// Subject vehicle info / 车辆参数部分
		OptionalDouble vehiVX; // [KPH] Subject vehicle's speed / 本车车速
		OptionalDouble vehiCurv; // [1/m] Subject vehicle's turning curvature / 本车曲率
		OptionalDouble vehiWidth; // [m] Subject vehicle's width / 本车宽度
		OptionalDouble vehiLength; // [m] Subject vehicle's length / 本车长度
		OptionalDouble vehiWheelBase; // [m] Subject vehicle's wheel base / 本车轴距
		OptionalDouble vehiFrontOverhang; // [m] Subject vehicle's front overhang / 本车前悬

		// FOV info / FOV信息
		Array<ObjectSensorFov> fovs; // List of FOV / FOV列表

		ObjectSensorSampleData()
		{}

		static Array<Point2D> calBoxCorners(ObjectInfo& obj, Double k) // 8 points are connected sequentially, and k is the reduction ratio of inclined points (generally 0.9~1) / 8个点顺序连接, k为斜方向点缩小比例(一般为0.9~1)
		{
			Double posx = obj.posx;
			Double posy = obj.posy;
			Double width = obj.width.value;
			Double length = obj.length.value;
			Double heading = obj.heading.valid ? obj.heading.value : 0;
			Float coef = (Float)k;

			Array<Point2D> pts(8);
			pts[0].x = (Float)(length / 2); pts[0].y = 0;
			pts[1].x = (Float)(length / 2) * coef; pts[1].y = (Float)(width / 2) * coef;
			pts[2].x = 0; pts[2].y = (Float)(width / 2);
			pts[3].x = (Float)(-length / 2) * coef; pts[3].y = (Float)(width / 2) * coef;
			pts[4].x = (Float)(-length / 2); pts[4].y = 0;
			pts[5].x = (Float)(-length / 2) * coef; pts[5].y = (Float)(-width / 2) * coef;
			pts[6].x = 0; pts[6].y = (Float)(-width / 2);
			pts[7].x = (Float)(length / 2) * coef; pts[7].y = (Float)(-width / 2) * coef;

			Double coshd = math::cos(heading);
			Double sinhd = math::sin(heading);
			for (UInt i = 0; i < 8; i++)
			{
				Point2D& pt = pts[i];
				Double x = coshd * pt.x - sinhd * pt.y;
				Double y = sinhd * pt.x + coshd * pt.y;
				pt.x = (Float)(posx + x);
				pt.y = (Float)(posy + y);
			}

			return pts;
		}

		OptionalDouble getTTC1(ObjectInfo& obj) // Calculate first order TTC / 计算一阶TTC [s]
		{
			if (obj.vxRel.valid)
			{
				double vxRelM = obj.vxRel.value / 3.6;

				if (obj.cpx > 0)
				{
					if (vxRelM >= -0.1) return 20;
					else return math::min(20.0, -(obj.cpx / vxRelM));
				}
				else if (vehiLength.valid && obj.cpx < -vehiLength.value)
				{
					if (vxRelM <= 0.1) return 20;
					else return math::min(20.0, (-vehiLength.value - obj.cpx) / vxRelM);
				}
				else return OptionalDouble();
			}
			else return OptionalDouble();
		}

		OptionalDouble getTTC2(ObjectInfo& obj) // Calculate second order TTC / 计算二阶TTC [s]
		{
			if (obj.vxRel.valid && obj.axRel.valid)
			{
				double vxRelM = obj.vxRel.value / 3.6;
				double axRel = obj.axRel.value;

				if (obj.cpx > 0)
				{
					if (vxRelM >= -0.1) return 20;
					if (math::abs(axRel) < 0.1) return math::min(20.0, -(obj.cpx / vxRelM));
					if (vxRelM * vxRelM - 2 * axRel * obj.cpx >= 0) return math::min(20.0, (-vxRelM - math::root(vxRelM * vxRelM - 2 * axRel * obj.cpx)) / axRel); // 0.5at^2+vt-d=0
					return 20;
				}
				else if (vehiLength.valid && obj.cpx < -vehiLength.value)
				{
					Double dist = -vehiLength.value - obj.cpx;

					if (vxRelM <= 0.1) return 20;
					else if (math::abs(axRel) < 0.1) return math::min(20.0, (-vehiLength.value - obj.cpx) / vxRelM);
					else if (vxRelM * vxRelM + 2 * axRel * dist >= 0) return math::min(20.0, (-vxRelM + math::root(vxRelM * vxRelM + 2 * axRel * dist)) / axRel);
					return 20;
				}
				else return OptionalDouble();
			}
			else return OptionalDouble();
		}

		OptionalDouble getTHW(ObjectInfo& obj) // Calculate time headway / 计算车间时距 [s]
		{
			if (obj.cpx > 0 && vehiVX.valid)
			{
				double vxEgoM = vehiVX.value / 3.6;
				if (vxEgoM <= 0.1) return 20;
				else return math::min(20.0, obj.cpx / vxEgoM);
			}
			else return OptionalDouble();
		}

		OptionalDouble getDCA(ObjectInfo& obj, Double reactionTime = 0) // Calculate DCA / 计算避免碰撞最小减速度 [m/s²] reactionTime=0: ODCA, >0: PDCA
		{
			if (obj.cpx > 0 && vehiVX.valid && obj.vxAbs.valid && obj.axAbs.valid)
			{
				Double vxEgoM = vehiVX.value / 3.6;
				Double vxAbsM = obj.vxAbs.value / 3.6;
				Double axAbs = obj.axAbs.value;

				Double egoDxReaction = vxEgoM * reactionTime;
				Double objDxReaction = vxAbsM * reactionTime + 0.5 * axAbs * reactionTime * reactionTime;

				Double dxRelAR = obj.cpx - 0.1 + objDxReaction - egoDxReaction;
				Double egoVxAbsAR = vxEgoM;
				Double objVxAbsAR = vxAbsM + axAbs * reactionTime;
				Double objVxRelAR = vxAbsM - vxEgoM;
				Double objAxAbsAR = axAbs;

				if (dxRelAR <= 0) return -20;

				if (objAxAbsAR >= 0.0)
				{
					if (objVxRelAR >= 0.0) return 0;
					else return math::max(-20.0, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
				}
				else
				{
					if (objVxRelAR >= 0.0) return math::max(-20.0, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
					if (objVxRelAR * objVxRelAR / (2 * dxRelAR) < objAxAbsAR) return math::max(-20.0, egoVxAbsAR * egoVxAbsAR / (objVxAbsAR * objVxAbsAR / objAxAbsAR - 2 * dxRelAR));
					else return math::max(-20.0, objAxAbsAR - objVxRelAR * objVxRelAR / (2 * dxRelAR));
				}
			}
			else return OptionalDouble();
		}

		OptionalDouble getOverlap(ObjectInfo& obj, Double boxCornerRatio = 0.98) // Calculate overlap / 计算重叠率 [%]
		{
			if (vehiCurv.valid && vehiWidth.valid)
			{
				Double widthEgo = vehiWidth.value;
				Double curvEgo = vehiCurv.value;
				Double fo = vehiFrontOverhang.valid ? vehiFrontOverhang.value : 0.9;
				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;

				if (obj.posx <= 0 || widthEgo <= 0) return OptionalDouble();

				Double widthObj;
				if (obj.posMode == PositionMode::ClosestPoint)
				{
					switch (obj.category())
					{
					case ObjectClass::Car:
						widthObj = 1.9;
						break;
					case ObjectClass::Ped:
						widthObj = 0.5;
						break;
					case ObjectClass::Truck:
						widthObj = 2.4;
						break;
					case ObjectClass::Bike:
						widthObj = 0.9;
						break;
					case ObjectClass::RailCar:
						widthObj = 2.6;
						break;
					default:
						widthObj = 1.0;
						break;
					}
				}
				else // BoxCenter
				{
					Array<Point2D> corners = calBoxCorners(obj, boxCornerRatio);
					Double miny = DINF, maxy = NDINF;
					for (UInt i = 0; i < corners.size(); i++)
					{
						miny = math::min(miny, (Double)corners[i].y);
						maxy = math::max(maxy, (Double)corners[i].y);
					}
					widthObj = maxy - miny;
				}

				Double dyEgo = 0.5 * curvEgo * obj.posx * obj.posx + curvEgo * (fo + wb) * obj.posx;
				Double dyObj = obj.posy;
				Double overLap = (math::min(dyEgo + 0.5 * widthEgo, dyObj + 0.5 * widthObj) - math::max(dyEgo - 0.5 * widthEgo, dyObj - 0.5 * widthObj));
				Double overLapRate = overLap / (overLap >= 0 ? math::min(widthEgo, widthObj) : widthEgo);
				return (int)(math::max(-3.0, overLapRate) * 100.0);
			}
			else return OptionalDouble();
		}

		OptionalDouble getLC(ObjectInfo& obj, Double boxCornerRatio = 0.98) // Calculate lateral clearance / 计算横向间距 [m]
		{
			if (vehiWidth.valid)
			{
				if (obj.posMode == PositionMode::ClosestPoint)
				{
					if (obj.posx > 0) return OptionalDouble();
					else if (obj.posy > 0) return obj.posy - vehiWidth.value * 0.5;
					else return -vehiWidth.value * 0.5 - obj.posy;
				}
				else // PM_BoxCenter
				{
					Array<Point2D> points = calBoxCorners(obj, boxCornerRatio);

					Double minx = DINF;
					Double miny = DINF, maxy = NDINF;
					for (UInt i = 0; i < points.size(); i++)
					{
						Point2D& pt = points[i];
						if (pt.x < minx) minx = pt.x;
						if (pt.y < miny) miny = pt.y;
						if (pt.y > maxy) maxy = pt.y;
					}

					if (minx > 0) return OptionalDouble();
					else if (miny > 0) return miny - vehiWidth.value * 0.5;
					else if (maxy < 0) return -vehiWidth.value * 0.5 - maxy;
					else return OptionalDouble();
				}
			}
			else return OptionalDouble();
		}

		OptionalDouble getRC(ObjectInfo& obj, Double boxCornerRatio = 0.98) // Calculate rear clearance / 计算后向间距 [m]
		{
			if (vehiLength.valid)
			{
				Array<Point2D> points;
				if (obj.posMode == PositionMode::ClosestPoint) points = Array<Point2D>::scalar(Point2D((Float)obj.posx, (Float)obj.posy));
				else points = calBoxCorners(obj, boxCornerRatio);

				Double maxx = NDINF;
				for (UInt i = 0; i < points.size(); i++)
				{
					Point2D& pt = points[i];
					if (pt.x > maxx) maxx = pt.x;
				}

				if (maxx > 0) return OptionalDouble();
				else return -vehiLength.value - maxx;
			}
			else return OptionalDouble();
		}
	};

	class ObjectSensorSample : public Object<class ObjectSensorSampleVars>
	{
	public:
		SPADAS_TYPE("obj_sensor_sample_v7.ObjectSensorSample")

		ObjectSensorSample();
		FullTimestamp& timestamp();
		ObjectSensorSampleData *operator ->();

		static String getProtocolName(UInt channel);
		SessionGeneralSample toGeneralSample();

		static Array<String> getProtocolNames(UInt channel);
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample);
	};
}

#endif
